#ifndef MPCCBF_H_
#define MPCCBF_H_

#include <math.h>
#include <vector>

#include "Eigen/Core"
#include "parameters.h"


namespace mpccbf {
    using Matrix = Eigen::MatrixXd;
    class MPCCBF{
        public:
        MPCCBF();
        ~MPCCBF();

        std::vector<double> Solve(const SystemState state, std::vector<TrajectoryPoint> planning_trajectoryPoint,
                                  const TrajectoryPoint target_point);
        protected:
            TrajectoryPoint QueryNearestPointByPos(const double x,const double y,std::vector<TrajectoryPoint> trajectory_points_);
            double PointDistanceSquare(const TrajectoryPoint &point, const double x, const double y);
            double NormalizeAngle(const double angle);
            
            /*ctrl time interval*/
            double ts_ = 0.0;

            const int state_size_ = 5;  //5个状态量
            const int controls_ = 3;    //acc_x, acc_y , yaw_w
            const int horizon_ = 10;     //预测时域

            // vehicle state matrix
            Matrix matrix_a_;
            // vehicle state matrix (discrete-time)
            Matrix matrix_ad_;
            // control matrix
            Matrix matrix_b_;
            // control matrix (discrete-time)
            Matrix matrix_bd_;
            // control authority weighting matrix
            Matrix matrix_r_;
            // state weighting matrix
            Matrix matrix_q_;
            // Endpoint state weight matrix
            Matrix matrix_p_;
            // 4 by 1 matrix; state matrix
            Matrix matrix_state_;
            // ref state matrix
            Matrix matrix_state_ref_;       

            //CBF gamma
            double CBF_Gamma = 0.1;

            double Max_X_Y_Range = 1e19; 
            double Max_Velocity = 0.0;
            double Max_YawRange = 0.0f;

            double Max_Acceleration = 0.0;
            double Max_UnitTurningAngle = 0.0;

    };
}
#endif
